#!/usr/bin/env roseus
;;(unix:putenv "ROS_MASTER_URI=http://pr1012:11311")

(ros::load-ros-manifest "jsk_pr2_gui")
(ros::roseus "nav_around_and_human_tracking")

(load "package://dsc_demo_main/dsc-nav.l")
(load "package://dsc_demo_main/dsc-action.l")

(defvar *tfl* (instance ros::transform-listener :init))
(defvar *tfb* (instance ros::transform-broadcaster :init))

(defun reset-demo ()
  (setq *time-count* (ros::time-now))
  (subscribe-human) (unsubscribe-status)
  (setq *human-detected* nil *status-detected* nil))

(defun demo-init-setting()
  (unix:putenv "ROBOT=HRP2WK")
  (setq *node-debug* nil)
  (case (unix::getenv "ROBOT")
    ('PR2
     (load "package://pr2eus/pr2-interface.l")
     (pr2) (setq *robot* *pr2*)
     (setq *ri* (instance pr2-interface :init)))
    ('HRP2WK
     ;;(unix:putenv "ROBOT=HRP2WK")
     (unix:putenv "HRP2NO=18")
     (load (format nil "~A/euslib/demo/h-yaguchi/hrp2vk/hand-functions.l"
		   (unix::getenv "CVSDIR")))
     (load (format nil "~A/hrp2/corba/orbixif.l" (unix::getenv "CVSDIR")))
     (load (format nil "~A/hrp2/plugins/plugins.l" (unix::getenv "CVSDIR")))
     ;;(load "../../present_motion/euslisp/hand-motion-hrp2vk.l")

     (hrp2vk-simple) (setq *robot* *hrp2*)
     (setq *ri* (eusorbix-init))
     (connect-hands)
     (send *ns* :controlmode-vector
	   (concatenate float-vector
			(make-array 5 :element-type float-vector :initial-element 0)
			(make-array 5 :element-type float-vector :initial-element 8)))
     (setq *open-av* (float-vector -65.0 -88.0 0.0 -40.0 0))
     ))
  (when *node-debug*
    (objects (list *robot*)))
  (ros::spin-once)
  (nav-init-setting)
  ;; (ros::subscribe "/face_detector/faces_cloud"
  ;;  		  sensor_msgs::PointCloud #'face-callback)
  (subscribe-human) ;;(subscribe-status)
  (ros::advertise "robotsound_jp" sound_play::SoundRequest 5)
  (setq *time-count* (ros::time-now))
  (setq *human-detected* nil *status-detected* nil)
  (ros::rate 10)
  )

(defun subscribe-human ()
  (ros::ros-error "subscribe-human")
  (ros::subscribe "/people_with_socks"
		  sensor_msgs::PointCloud #'body-callback-init))
(defun unsubscribe-human ()
  (ros::ros-error "unsubscribe-human")
  (ros::unsubscribe "/people_with_socks"))

(defun subscribe-status ()
  (ros::ros-error "subscribe-status")
  (ros::subscribe "/state_result"
		  roseus::StringStamped #'status-callback))
(defun unsubscribe-status ()
  (ros::ros-error "unsubscribe-status")
  (ros::unsubscribe "/state_result"))

(defun is-old-msg (msg)
  (let ((htime (send msg :header :stamp)))
    (cond ((> 0 (send (ros::time- htime *time-count*) :to-sec))
	   (ros::ros-info "time-check: message is old, return with no action")
	   t))))

(defun status-callback (msg)
 (if (is-old-msg msg) (return-from status-callback nil))
 (let ((data (send msg :data)))
   (ros::ros-warn "status-callback")
   (print data)
   ;;(string= (send msg :data) "both")
   (cond ((eq 'both (read-from-string data))
	  (ros::ros-warn "status-detected")
	  (setq *status-detected* t))
   )))

;; (defun face-callback(msg)
;;   (return-from face-callback nil);;;; we only use body callback if(kinect)
;;   (when (null (send msg :points))
;;     (ros::ros-warn "face-callback: no points.")
;;     (return-from face-callback nil))
;;   (ros::ros-info "face-callback[0]: ~A" (ros::tf-point->pos (elt (send msg :points) 0)))

;;   (send *pr2* :angle-vector (send *ri* :state :potentio-vector))
;;   (ros::ros-debug "face-callback[0]:~A" (send (send (send *pr2* :wide_stereo_gazebo_l_stereo_camera_optical_frame) :translate (ros::tf-point->pos (elt (send msg :points) 0))) :worldpos));;

;;     (setq *place* (send (send (send (elt (send *pr2* :cameras) 2) :copy-worldcoords) :translate (ros::tf-point->pos (elt (send msg :points) 0))) :worldpos))
;;     (when *node-debug*
;;       (objects (list *pr2* (make-cube 100 100 100 :pos *place*))))
;;     (send *ri* :go-pos (/ (- (elt *place* 0) 500) 1000) (/ (elt *place* 1) 1000) 0)
;;   (ros::ros-info "face-callback: end"))


;; the frame_id of the pointclouds is expected to be "base_footprint"
(defun body-callback-init (msg)
  (if (is-old-msg msg) (return-from body-callback-init nil))
  (speak-jp "はっけん")
  (ros::ros-info "body-cb-init")
  (setq *human-msg* msg)
  (setq *human-detected* t))

(defun body-callback (msg)
  (when (null (send msg :points))
    (ros::ros-warn "body-callback: no points.")
    (return-from body-callback nil))
  (let* ((points0 (elt (send msg :points) 0))
	 (cds0 (ros::tf-point->pos points0))
	 (human0 (float-vector (aref cds0 0) (aref cds0 1) 0)))
    (ros::ros-info "body-callback[0]: ~A" cds0)
    (if (and (> (aref cds0 0) 0) (< (aref cds0 0) 0))
	(progn (ros::ros-warn "nan detected")
	       (return-from body-callback nil)))

    (send *robot* :angle-vector (send *ri* :state :potentio-vector));
    (when *node-debug*
      (objects (list *robot* (make-cube 100 100 100 :pos human0))));

    (print (list 'human0-pose human0))

    (ros::warn "body-callback:found human, reaching...~%")
    ;; (send *ri* :move-to (orient-coords-to-axis
    ;;   (send *pr2* :copy-worldcoords) (v- human0 (send *pr2* :worldpos)) :x))
    (let ((dif-direction (normalize-vector
			  (v- human0 (send *robot* :worldpos)))))
      (ros::ros-info "body-callback: turn")
      (send *ri* :go-pos-unsafe 0 0
	    (* (if (> (elt dif-direction 1) 0) 1 -1)
	       (rad2deg (acos (v. (normalize-vector
				   (send *robot* :rotate-vector (float-vector 1 0 0))) dif-direction)))))
      (ros::ros-info "body-callback go straight")
      (print (/ (- (norm human0) 500) 1000))
      (send *ri* :go-pos-unsafe (/ (- (norm human0) 1000) 1000) 0 0)
      )
  ))

(defun wait-for-hand-over ()
  (case (unix::getenv "ROBOT")
    ('PR2
     (pr2-tuckarm-pose)
     (pr2-grasp-choco-pose)
     (unix:sleep 1)
     (pr2-present-motion)
     (ros::ros-error "wait-for-hand-over")
     (subscribe-status)
     (setq *status-detected* nil)
     (while (null *status-detected*)
       (ros::spin-once)
       (ros::sleep)
       (ros::ros-info "waiting status"))
     (unsubscribe-status)
     (pr2-release-choco))
    ('HRP2WK
     (hrp2-grasp-present)
     (ros::ros-error "wait-for-hand-over")
     (subscribe-status)
     (setq *status-detected* nil)
     (while (null *status-detected*)
       (ros::spin-once)
       (ros::sleep)
       (ros::ros-info "waiting status"))
     (unsubscribe-status)
     (hrp2-release-choco))
    ))

(demo-init-setting)

(warn "(move-to-spot-sequence *spots-list*)~%")

